/***************************************************************************//**
 * @file MSc_Location.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_LOCATION_CPP
#define MSC_LOCATION_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#include <cmath>

#include <utils/MSc_Angles.h>
#include <utils/MSc_Location.h>

////////////////////////////////////////////////////////////////////////////////

const double MSc_Location::m_a = 6378137.0;
const double MSc_Location::m_f = 1.0 / 298.257223563;

const double MSc_Location::m_b       = 6356752.3142;
const double MSc_Location::m_a2      = MSc_Location::m_a * MSc_Location::m_a;
const double MSc_Location::m_b2      = MSc_Location::m_b * MSc_Location::m_b;
const double MSc_Location::m_e2      = 6.6943799901400e-3;
const double MSc_Location::m_e       = 8.1819190842622e-2;
const double MSc_Location::m_eprim2  = 6.7394967422800e-3;
const double MSc_Location::m_eprim   = 8.2094437949696e-2;

const double MSc_Location::m_GM      = 3986004.418e8;
const double MSc_Location::m_omega   = 7.2921151467e-5;
const double MSc_Location::m_gamma_e = 9.7803253359;
const double MSc_Location::m_gamma_p = 9.8321849378;
const double MSc_Location::m_k       = 0.00193185265241;
const double MSc_Location::m_m       = 0.00344978650684;

////////////////////////////////////////////////////////////////////////////////

MSc_Location::MSc_Location()
{
    setGeo( 0.0, 0.0, 0.0 );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Location::MSc_Location( const MSc_Location &loc )
{
    m_loc_WGS = loc.m_loc_WGS;

    m_lat = loc.m_lat;
    m_lon = loc.m_lon;
    m_alt = loc.m_alt;

    m_norm_WGS = loc.m_norm_WGS;
    m_grav_WGS = loc.m_grav_WGS;

    m_T_ned2wgs = loc.m_T_ned2wgs;
    m_T_wgs2ned = loc.m_T_wgs2ned;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Location MSc_Location::getFromGeo( double lat, double lon, double alt )
{
    MSc_Location result;

    result.setGeo( lat, lon, alt );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Location MSc_Location::getFromWGS( double x, double y, double z )
{
    MSc_Location result;

    result.setWGS( x, y, z );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Location MSc_Location::getFromWGS( const MSc_Vector3 &vect )
{
    MSc_Location result;

    result.setWGS( vect );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Vector3 MSc_Location::getOmega_WGS()
{
    return MSc_Vector3::getFrom( 0.0, 0.0, m_omega );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::geo2wgs( double lat, double lon, double alt, double &x, double &y, double &z )
{
    double sinLat = sin( lat );
    double cosLat = cos( lat );
    double sinLon = sin( lon );
    double cosLon = cos( lon );

    double n = m_a / sqrt( 1.0 - m_e2 * sinLat*sinLat );

    x = ( n + alt ) * cosLat * cosLon;
    y = ( n + alt ) * cosLat * sinLon;
    z = ( n * ( m_b2 / m_a2 ) + alt ) * sinLat;
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::geo2wgs( double lat, double lon, double alt, MSc_Vector3 &vect )
{
    MSc_Location::geo2wgs( lat, lon, alt, vect.x(), vect.y(), vect.z() );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::wgs2geo( double x, double y, double z, double &lat, double &lon, double &alt )
{
    // calculating latitude, longitude and altitude
    // see: Zhu J., "Conversion of Earth-centered Earth-fixed coordinates to geodetic coordinates"

    double z2 = z*z;
    double r  = sqrt( x*x + y*y );
    double r2 = r*r;
    double e2 = m_a2 - m_b2;
    double f  = 54.0 * m_b2 * z2;
    double g  = r2 + ( 1.0 - m_e2 )*z2 - m_e2*e2;
    double c  = m_e2*m_e2 * f * r2 / ( g*g*g );
    double s  = pow( 1.0 + c + sqrt( c*c + 2.0*c ), 1.0/3.0 );
    double p0 = s + 1.0/s + 1.0;
    double p  = f / ( 3.0 * p0*p0 * g*g );
    double q  = sqrt( 1.0 + 2.0*( m_e2*m_e2 )*p );
    double r0 = -( p * m_e2 * r )/( 1.0 + q ) + sqrt( 0.5*m_a2*( 1.0 + 1.0/q ) - p*( 1.0 - m_e2 )*z2/( q + q*q ) - 0.5*p*r2 );
    double uv = r - m_e2*r0;
    double u  = sqrt( uv*uv + z2 );
    double v  = sqrt( uv*uv + ( 1.0 - m_e2 )*z2 );
    double z0 = m_b2 * z / ( m_a * v );

    alt = u * ( 1.0 - m_b2 / ( m_a * v ) );
    lat = atan( ( z + m_eprim2*z0 )/r );
    lon = atan2( y, x );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::wgs2geo( const MSc_Vector3 &vect, double & lat, double & lon, double & alt )
{
    MSc_Location::wgs2geo( vect.x(), vect.y(), vect.z(), lat, lon, alt );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::setGeo( double lat, double lon, double alt )
{
    m_lat = lat;
    m_lon = lon;
    m_alt = alt;

    MSc_Location::geo2wgs( m_lat, m_lon, m_alt, m_loc_WGS );

    update();
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::setWGS( double x, double y, double z )
{
    m_loc_WGS.x() = x;
    m_loc_WGS.y() = y;
    m_loc_WGS.z() = z;

    MSc_Location::wgs2geo( m_loc_WGS, m_lat, m_lon, m_alt );

    update();
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::setWGS( const MSc_Vector3 &vect )
{
    setWGS( vect( 0 ), vect( 1 ), vect( 2 ) );
}

////////////////////////////////////////////////////////////////////////////////

void MSc_Location::update()
{
    double cosLat = cos( m_lat );
    double cosLon = cos( m_lon );
    double sinLat = sin( m_lat );
    double sinLon = sin( m_lon );

    double sinLat2 = sinLat*sinLat;

    // normal to ellipsoid
    m_norm_WGS( 0 ) = cosLat * cos( m_lon );
    m_norm_WGS( 1 ) = cosLat * sin( m_lon );
    m_norm_WGS( 2 ) = sin( m_lat );

    // gravity formula (NIMA TR-8350.2 - Department of Defence World Geodetic System 1984, p. 4-2)
    double gamma_0 = m_gamma_e * ( 1.0 + m_k * sinLat2 ) / sqrt( 1.0 - m_e2 * sinLat2 );
    double gamma_h = gamma_0 * ( 1.0 - (2.0/m_a)*(1.0 + m_f + m_m - 2.0*m_f*sinLat2)*m_alt + (3.0/m_a2)*m_alt*m_alt );

    m_grav_WGS = - gamma_h * m_norm_WGS;

    // rotation matricies
    double T_ned2wgs[ 9 ];
    double T_wgs2ned[ 9 ];

    // NED to WGS
    T_ned2wgs[ 0 ] = -cosLon*sinLat;
    T_ned2wgs[ 1 ] = -sinLon;
    T_ned2wgs[ 2 ] = -cosLon*cosLat;

    T_ned2wgs[ 3 ] = -sinLon*sinLat;
    T_ned2wgs[ 4 ] =  cosLon;
    T_ned2wgs[ 5 ] = -sinLon*cosLat;

    T_ned2wgs[ 6 ] =  cosLat;
    T_ned2wgs[ 7 ] =  0.0;
    T_ned2wgs[ 8 ] = -sinLat;

    m_T_ned2wgs.setArray( T_ned2wgs );

    // WGS to NED
    T_wgs2ned[ 0 ] = -cosLon * sinLat;
    T_wgs2ned[ 1 ] = -sinLon * sinLat;
    T_wgs2ned[ 2 ] =  cosLat;

    T_wgs2ned[ 3 ] = -sinLon;
    T_wgs2ned[ 4 ] =  cosLon;
    T_wgs2ned[ 5 ] =  0.0;

    T_wgs2ned[ 6 ] = -cosLon * cosLat;
    T_wgs2ned[ 7 ] = -sinLon * cosLat;
    T_wgs2ned[ 8 ] = -sinLat;

    m_T_wgs2ned.setArray( T_wgs2ned );
}
